/*
 *  TestDriver.h
 *  powaah
 *
 *  Created by Tim Uusitalo on 3/30/11.
 *  Copyright 2011 __MyCompanyName__. All rights reserved.
 *
 */

#ifndef TEST_DRIVER_H

#define TEST_DRIVER_H

#include "WrapperBaseDriver.h"

#include "CarState.h"


#include "CarControl.h"

#include <iostream>

#include "CurveAgent.h"
#include "Timer.h"

#include "SpeedAgent.h"

#include "TrackAgent.h"

#include "DriverAgent.h"

#include "TrackLearner.h"

class TestDriver : public WrapperBaseDriver
{
public: 
	TestDriver();
	~TestDriver();
	
	// the drive function wiht string input and output
	string drive(string sensors);
	
	// drive function that exploits the CarState and CarControl wrappers as input and output.
	CarControl wDrive(CarState cs);

		
	void onShutDown();
	
	void onRestart();
	
	void init (float* const angles);
	
	
private:
	
	const float getSteer(CarState& cs);
	
	const float getAccelPad(CarState& cs, CarState& old);
	
	const float getBrake(CarState& cs, CarState& old);
	
	const int getGear(CarState& cs);
	
	void clutching(CarState& cs, float& clutch);
	
	const float getAllowedSpeed(CarState& cs);
	
	const float filterABS(CarState& cs, float brake);
	
	const float filterTLS(CarState& cs, const float accel);
	
private:
	
	
	float angles[20];
	float enemy_angles[36];
	
	// RPM values to change gear 
	static const int gearUp[6];
	static const int gearDown[6];

	/* Stuck constants*/
	
	// How many time steps the controller wait before recovering from a stuck position
	static const int stuckTime;
	// When car angle w.r.t. track axis is grather tan stuckAngle, the car is probably stuck
	static const float stuckAngle;
	
	/* Steering constants*/
	
	// Angle associated to a full steer command
	static const float steerLock;	
	// Min speed to reduce steering command 
	static const float steerSensitivityOffset;
	// Coefficient to reduce steering command at high speed (to avoid loosing the control)
	static const float wheelSensitivityCoeff;
	
	/* Accel and Brake Constants*/
	
	// max speed allowed
	static const float maxSpeed;
	// Min distance from track border to drive at  max speed
	static const float maxSpeedDist;
	// pre-computed sin5
	static const float sin5;
	// pre-computed cos5
	static const float cos5;
	
	/* ABS Filter Constants */
	
	// Radius of the 4 wheels of the car
	static const float wheelRadius[4];
	// min slip to prevent ABS
	static const float absSlip;						
	// range to normalize the ABS effect on the brake
	static const float absRange;
	// min speed to activate ABS
	static const float absMinSpeed;
	
	/* Clutch constants */
	static const float clutchMax;
	static const float clutchDelta;
	static const float clutchRange;
	static const float clutchDeltaTime;
	static const float clutchDeltaRaced;
	static const float clutchDec;
	static const float clutchMaxModifier;
	static const float clutchMaxTime;
	
	// counter of stuck steps
	int stuck;
	
	// current clutch
	float clutch;
	
	CarState old_state;
	CarControl old_cc;
	Timer timer;
	
	float offset;
	
	float dt;
	

	DriverAgent da;
	
	float ds;
	
	TrackLearner track;
	
	int lap;
	
	int tick;
	
	float time;
	float start_velocity;
	int lastrpm;
	
	std::vector<float> accelerations;
	
	float resulting_devolver;
	
};


#endif
